package org.hs.phd.odi.tracking.tracker.trackwindow;

import java.awt.Polygon;
import java.awt.geom.Point2D;
import java.awt.geom.Point2D.Float;
import java.awt.geom.Rectangle2D.Double;

import org.hs.phd.common.geometry.DistanceNormKit;
import org.hs.phd.common.geometry.Norm2D;
import org.hs.phd.common.improc.traversal.PixelTraversalKit;
import org.hs.phd.common.improc.traversal.RGBImagePixelProcessor;
import org.hs.phd.common.improc.traversal.RGBImagePixelTraverser;
import org.hs.phd.odi.tracking.tracker.TrackWindow;
import org.hs.phd.odi.transformation.ODITransformation;
import org.hs.phd.odi.transformation.ODIUtils;

import com.googlecode.javacv.cpp.opencv_core.IplImage;

public class ODIOnTheSphereTrackWindow implements TrackWindow {
	
	private static final double TWO_PI = 2*Math.PI;
	private static final double HALF_PI = Math.PI/2;

	private double theta;
	private double phi;
	private double deltaTheta;
	private double deltaPhi;
	private final ODITransformation transformation;
	private Norm2D geodesicNorm;

	public ODIOnTheSphereTrackWindow( ODITransformation transformation, double theta, double phi, double deltaTheta, double deltaPhi){
		this.transformation = transformation;
		set(theta, phi, deltaTheta, deltaPhi);
		geodesicNorm = DistanceNormKit.getGeodesicNorm();
	}
	
	public ODIOnTheSphereTrackWindow(ODITransformation transformation) {
		this(transformation, 0,0,0,0);
	}

	@Override
	public Polygon getAsPolygon() {
		
		int polygonPointCountForAnArc = 10;
		int npoints = 2*polygonPointCountForAnArc;
		int[] xpoints = new int[npoints];
		int[] ypoints = new int[npoints];
		
		double minPhi = phi - deltaPhi;
		double phiStep = 2*deltaPhi/polygonPointCountForAnArc;
		
		int pIndex = 0;
		
		
		double upperArcTheta = theta + deltaTheta;
		double lowerArcTheta = theta - deltaTheta;
		
		double currentPhi = minPhi;
		
		//upper arc
		for(int i = 0; i < polygonPointCountForAnArc; i++){
			Point2D.Double imagePointArc = transformation.sphereToImage(upperArcTheta, currentPhi);
			xpoints[pIndex] = (int) Math.round(imagePointArc.x);
			ypoints[pIndex] = (int) Math.round(imagePointArc.y);
			currentPhi += phiStep;
			pIndex++;
		}
		
		//lower arc
		for(int i = polygonPointCountForAnArc - 1; i >= 0; i--){
			currentPhi -= phiStep;
			Point2D.Double imagePointArc = transformation.sphereToImage(lowerArcTheta, currentPhi);
			xpoints[pIndex] = (int) Math.round(imagePointArc.x);
			ypoints[pIndex] = (int) Math.round(imagePointArc.y);
			pIndex++;
		}
		
		Polygon polygon = new Polygon(xpoints, ypoints, npoints);
		return polygon;
	}

	@Override
	public void moveToBestMatchingLocationFor(Polygon initialTrackShape) {
		
		Double boundsOnTheSphere = ODIUtils.getBoundsOnTheSphere(transformation, initialTrackShape);

		theta = boundsOnTheSphere.x + boundsOnTheSphere.width/2;
		phi = boundsOnTheSphere.y + boundsOnTheSphere.height/2;
		deltaTheta = boundsOnTheSphere.width/2;
		deltaPhi = boundsOnTheSphere.height/2;
	}

	@Override
	public TrackWindow getCopy() {
		return new ODIOnTheSphereTrackWindow(transformation, theta, phi, deltaTheta, deltaPhi);
	}

	@Override
	public void moveCenterToPosition(Point2D.Float newPosition) {
		Point2D.Double newSphericalCoords = transformation.imageToSphere(newPosition.x, newPosition.y);
		theta = newSphericalCoords.x;
		phi = newSphericalCoords.y;
	}

	@Override
	public Point2D.Float getCenter() {
		Point2D.Double centerOnImage = transformation.sphereToImage(theta, phi);

		return new Point2D.Float((float)centerOnImage.x, (float)centerOnImage.y);
	}

	@Override
	public void traversePixels(IplImage frame, RGBImagePixelProcessor processor) {
		RGBImagePixelTraverser traverser = PixelTraversalKit.createRGBPolygonalTraverser(getAsPolygon());
		traverser.traversePixels(frame, processor);
	}

	@Override
	public Float getGridCenter() {
		return getCenter();
	}

	@Override
	public float distanceFromCenterTo(float x, float y) {
		Float gridCenter = getCenter();
		float dx = x - gridCenter.x;
		float dy = y - gridCenter.y;
		return (float)Math.sqrt(dx*dx + dy*dy);
	}

	@Override
	public float normalizedDistanceFromGridCenterTo(float x, float y) {
		Point2D.Double pointOnSphere = transformation.imageToSphere(x, y);
		
		double lon1 = phiToLon(phi);
		double lat1 = thetaToLat(theta);
		double lon2 = phiToLon(pointOnSphere.y);
		double lat2 = thetaToLat(pointOnSphere.x);

		double geodesicDistance = geodesicNorm.distance(lon1, lat1, lon2, lat2);
		
		double maxDistanceAllowed = Math.max(deltaPhi, deltaTheta);
		double normalizedDistance = geodesicDistance / maxDistanceAllowed*4/5;
		
		return (float) normalizedDistance;
	}

	public static double thetaToLat(double theta) {
		return theta < HALF_PI ? HALF_PI - theta : theta - HALF_PI;
	}

	public static double phiToLon(double phi) {
		return phi < Math.PI ? phi : TWO_PI - phi;
	}

	public double getTheta() {
		return theta;
	}

	public double getPhi() {
		return phi;
	}

	public double getDeltaTheta() {
		return deltaTheta;
	}

	public double getDeltaPhi() {
		return deltaPhi;
	}

	public void set(double theta, double phi, double deltaTheta, double deltaPhi){
		this.theta = theta;
		this.phi = phi;
		this.deltaTheta = deltaTheta;
		this.deltaPhi = deltaPhi;
	}

	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		long temp;
		temp = java.lang.Double.doubleToLongBits(deltaPhi);
		result = prime * result + (int) (temp ^ (temp >>> 32));
		temp = java.lang.Double.doubleToLongBits(deltaTheta);
		result = prime * result + (int) (temp ^ (temp >>> 32));
		result = prime * result + ((geodesicNorm == null) ? 0 : geodesicNorm.hashCode());
		temp = java.lang.Double.doubleToLongBits(phi);
		result = prime * result + (int) (temp ^ (temp >>> 32));
		temp = java.lang.Double.doubleToLongBits(theta);
		result = prime * result + (int) (temp ^ (temp >>> 32));
		result = prime * result + ((transformation == null) ? 0 : transformation.hashCode());
		return result;
	}

	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		ODIOnTheSphereTrackWindow other = (ODIOnTheSphereTrackWindow) obj;
		if (java.lang.Double.doubleToLongBits(deltaPhi) != java.lang.Double.doubleToLongBits(other.deltaPhi))
			return false;
		if (java.lang.Double.doubleToLongBits(deltaTheta) != java.lang.Double.doubleToLongBits(other.deltaTheta))
			return false;
		if (geodesicNorm == null) {
			if (other.geodesicNorm != null)
				return false;
		} else if (!geodesicNorm.equals(other.geodesicNorm))
			return false;
		if (java.lang.Double.doubleToLongBits(phi) != java.lang.Double.doubleToLongBits(other.phi))
			return false;
		if (java.lang.Double.doubleToLongBits(theta) != java.lang.Double.doubleToLongBits(other.theta))
			return false;
		if (transformation == null) {
			if (other.transformation != null)
				return false;
		} else if (!transformation.equals(other.transformation))
			return false;
		return true;
	}
	
	@Override
	public String toString() {
		
		return "[" 
				+ Math.toDegrees(theta - deltaTheta) + ","
				+ Math.toDegrees(phi - deltaPhi) + ","
				+ Math.toDegrees(theta + deltaTheta) + ","
				+ Math.toDegrees(phi + deltaPhi) 
				+ "]";
	}


}
